
#include <iostream>
using namespace std;
#include "serial.h"

#define POORT 0
#define MAXV 0x40


void Drive(int snelheid, int direction)
{
	int res = OpenComport(POORT, 19200);
	float speed = MAXV*snelheid*0.01;
	if(res == 1)
	{
		printf("Error while opening comport\n");
		return;
	}

	unsigned char buf[3];
	buf[0]=(direction) ? 0xD6 : 0xD9; //0xD9 = forward, 0xD6 is achteruit
	buf[1] = speed;		
	buf[2] = speed;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
	{
		if(snelheid == 0)
			printf("Stopped\n");
		else
			printf("Going forward, speed %d\n", (int)speed);
	}
	else
		printf("Error while trying to move.\n");
	CloseComport(POORT);
}

void Rotate(int hoek, bool clockwise, int snelheid)
{
	int res = OpenComport(POORT, 19200);
	float speed = MAXV*snelheid*0.01;
	
	if(res == 1)
	{
		printf("Error while opening comport\n");
		return;
	}

	unsigned char buf[3];

	if(clockwise)
		buf[0] = 0xD5;
	else
		buf[0] = 0xDA;
	buf[1] = speed;		//0xD9 = forward, 0xD6 is achteruit
	buf[2] = speed;
	res = SendBuf(POORT, buf, 3);
	if(res != 1)
		printf("Rotating...\n");
	else
		printf("Error while trying to rotate.\n");
	CloseComport(POORT);
}



// serial test function

int main(int argc, char *argv[])
{
	int i;
	Drive(50,0);
	sleep(1);
	Drive(0,0);
	for(i=0;i<10;i++)
	{
		Rotate(30,1,50);
		usleep(500000);	
		Rotate(30,0,50);
		usleep(500000);	
	}
	Drive(0,0);
	
	return 0;
}
